package com.crunch.physics;

import com.crunch.math.Vector2f;

import java.util.*;

/**
 * The physics simulation.
 */
public class PhysicsSimulation {
	public static class Settings {
		public float gridCellSize;      // size of grid cells in the hash grid
		public float contactThreshold;  // threshold for contacts
	}

	public PhysicsSimulation(Settings settings) {
		if (settings.gridCellSize <= 0.0f) {
			throw new IllegalArgumentException("Non-positive grid cell size");
		}
		if (settings.contactThreshold <= 0.0f) {
			throw new IllegalArgumentException("Non-positive contact threshold");
		}

		this.settings = new Settings();
		this.settings.gridCellSize = settings.gridCellSize;
		this.settings.contactThreshold = settings.contactThreshold;

		evaluator = new PhysicsEvaluator(this);

		simulationLock = false;
		movableEntityLock = false;

		staticColliders = new ArrayList<Segment>();
		staticColliderEntities = new ArrayList<HashGrid.Entity>();
		dynamicEntities = new HashSet<DynamicPhysicsEntity>();
		movableEntities = new HashSet<MovablePhysicsEntity>();
		proximityZones = new HashSet<PhysicsProximityZone>();

		hashGrid = new HashGrid(settings.gridCellSize);

		dynamicEntityAddQueue = new ArrayList<DynamicPhysicsEntity>();
		movableEntityAddQueue = new ArrayList<MovablePhysicsEntity>();
		proximityZoneAddQueue = new ArrayList<PhysicsProximityZone>();

		dynamicEntityDeleteQueue = new ArrayList<DynamicPhysicsEntity>();
		movableEntityDeleteQueue = new ArrayList<MovablePhysicsEntity>();
		proximityZoneDeleteQueue = new ArrayList<PhysicsProximityZone>();

		// allocate CATEGORIES x CATEGORIES array of collision masks and event handlers
		categoryCollisionMasks = new BitSet(DynamicPhysicsEntity.CATEGORIES * DynamicPhysicsEntity.CATEGORIES);
		collisionEventHandlers =
				new CollisionEventHandler[DynamicPhysicsEntity.CATEGORIES][DynamicPhysicsEntity.CATEGORIES];

		trackedIntersections = new HashMap<TrackedIntersection, TrackedIntersectionListNode>();
	}

	////////
	// adding objects to simulation:
	// objects added during simulation step are added at very end

	// should be added before simulation begins
	public void addStaticColliders(Segment[] segments) {
		if (simulationLock || raycastLock) {
			throw new IllegalStateException("Simulation is locked");
		}

		int newSize = staticColliders.size() + segments.length;
		staticColliders.ensureCapacity(newSize);
		staticColliderEntities.ensureCapacity(newSize);
		Vector2f aabbMin = new Vector2f();
		Vector2f aabbMax = new Vector2f();
		for (Segment s : segments) {
			int index = staticColliders.size();
			staticColliders.add(s);
			aabbMin.set(
					Math.min(s.getVerticesDirect()[0].x(), s.getVerticesDirect()[1].x()) - settings.contactThreshold,
					Math.min(s.getVerticesDirect()[0].y(), s.getVerticesDirect()[1].y()) - settings.contactThreshold);
			aabbMax.set(
					Math.max(s.getVerticesDirect()[0].x(), s.getVerticesDirect()[1].x()) + settings.contactThreshold,
					Math.max(s.getVerticesDirect()[0].y(), s.getVerticesDirect()[1].y()) + settings.contactThreshold);
			// the "object" data is an Integer containing the collider's index
			staticColliderEntities.add(hashGrid.addEntity(aabbMin, aabbMax, GRID_STATIC_COLLIDER_TYPE, false, index));
		}
	}

	// removes and shifts indices down
	public void removeStaticColliders(int[] indices) {
		if (simulationLock || raycastLock) {
			throw new IllegalStateException("Simulation is locked");
		}

		for (int index : indices) {
			staticColliderEntities.get(index).remove();
			staticColliderEntities.set(index, null);
		}

		// remove null slots
		int next = 0;
		for (int i = 0; i < staticColliderEntities.size(); ++i) {
			if (staticColliderEntities.get(i) != null) {
				staticColliderEntities.set(next, staticColliderEntities.get(i));
				staticColliders.set(next, staticColliders.get(i));
				++next;
			}
		}

		while (staticColliderEntities.size() > next) {
			staticColliderEntities.remove(staticColliderEntities.size()-1);
			staticColliders.remove(staticColliders.size()-1);
		}

		// fix IDs in hash grid entities
		for (int i = 0; i < staticColliderEntities.size(); ++i) {
			staticColliderEntities.get(i).setData(i);
		}
	}

	public int getStaticColliderCount() {
		return staticColliders.size();
	}

	public Segment getStaticCollider(int index) {
		return staticColliders.get(index);
	}

	public DynamicPhysicsEntity addDynamicEntity(DynamicPhysicsEntity.Description description) {
		return new DynamicPhysicsEntity(this, description);
	}

	public Set<DynamicPhysicsEntity> getDynamicEntities() {
		return Collections.unmodifiableSet(dynamicEntities);
	}

	public MovablePhysicsEntity addMovableEntity(MovablePhysicsEntity.Description description) {
		return new MovablePhysicsEntity(this, description);
	}

	public Set<MovablePhysicsEntity> getMovableEntities() {
		return Collections.unmodifiableSet(movableEntities);
	}

	public PhysicsProximityZone addProximityZone(PhysicsProximityZone.Description description) {
		return new PhysicsProximityZone(this, description);
	}

	public Set<PhysicsProximityZone> getProximityZones() {
		return Collections.unmodifiableSet(proximityZones);
	}

	////////
	// defining collision behavior

	public interface CollisionEventHandler {
		public void onCollision(DynamicPhysicsEntity entityA, DynamicPhysicsEntity entityB);
	}

	// sets whether collisions between entities of categoryA and categoryB are enabled
	// note: disabled collisions still trigger category-specific handlers
	public void setCategoryCollisionsEnabled(int categoryA, int categoryB, boolean enabled) {
		if (categoryA < 0 || categoryB < 0 ||
				categoryA >= DynamicPhysicsEntity.CATEGORIES || categoryB >= DynamicPhysicsEntity.CATEGORIES) {
			throw new IllegalArgumentException("Invalid category");
		}

		// mask is symmetrical, so only use cases where A <= B
		if (categoryA > categoryB) {
			int temp = categoryA;
			categoryA = categoryB;
			categoryB = temp;
		}

		categoryCollisionMasks.set(categoryB * DynamicPhysicsEntity.CATEGORIES + categoryA, enabled);
	}

	public boolean isCategoryCollisionEnabled(int categoryA, int categoryB) {
		if (categoryA < 0 || categoryB < 0 ||
				categoryA >= DynamicPhysicsEntity.CATEGORIES || categoryB >= DynamicPhysicsEntity.CATEGORIES) {
			throw new IllegalArgumentException("Invalid category");
		}

		// mask is symmetrical, so only use cases where A <= B
		if (categoryA > categoryB) {
			int temp = categoryA;
			categoryA = categoryB;
			categoryB = temp;
		}

		return categoryCollisionMasks.get(categoryB * DynamicPhysicsEntity.CATEGORIES + categoryA);
	}

	// associates a collision handler with the pair of categories
	// if a handler is set for (categoryA, categoryB), then when entityA of categoryA collides with entityB of categoryB
	// onCollision(entityA, entityB) is called
	// the order of entities passed into the handler matches the order categories are specified
	// a handler for (categoryB, categoryA) will overwrite any handler for (categoryA, categoryB)
	public void setCategoryCollisionHandler(int categoryA, int categoryB, CollisionEventHandler collisionEventHandler) {
		if (categoryA < 0 || categoryB < 0 ||
				categoryA >= DynamicPhysicsEntity.CATEGORIES || categoryB >= DynamicPhysicsEntity.CATEGORIES) {
			throw new IllegalArgumentException("Invalid category");
		}

		// clear flipped handler (if it exists)
		collisionEventHandlers[categoryB][categoryA] = null;
		collisionEventHandlers[categoryA][categoryB] = collisionEventHandler;
	}

	// returns collision handler associated with the pair of categories
	// note: if a handler was registered with the categories flipped, it will NOT be returned
	// this is so that handler category order can be distinguished
	// to reliably check whether a handler exists, check both category orderings
	public CollisionEventHandler getCategoryCollisionHandler(int categoryA, int categoryB) {
		if (categoryA < 0 || categoryB < 0 ||
				categoryA >= DynamicPhysicsEntity.CATEGORIES || categoryB >= DynamicPhysicsEntity.CATEGORIES) {
			throw new IllegalArgumentException("Invalid category");
		}

		return collisionEventHandlers[categoryA][categoryB];
	}

	////////
	// raycasting

	public void castRay(PhysicsRaycast raycast) {
		evaluator.castRay(raycast);
	}

	////////
	// stepping simulation

	public void stepSimulation(float dt) {
		evaluator.stepSimulation(dt);
	}

	// settings for this simulation
	Settings settings;

	// evaluator for simulation
	PhysicsEvaluator evaluator;

	// locks on the simulation
	boolean simulationLock;             // indicates that the simulation is running
	boolean movableEntityLock;          // set to true at times when movable entities should not be modified
	boolean trackedIntersectionLock;    // locked when tracked intersections are being iterated
	boolean raycastLock;                // locked when raycast is occurring

	// sets of entities in simulation
	ArrayList<Segment> staticColliders;
	ArrayList<HashGrid.Entity> staticColliderEntities;
	HashSet<DynamicPhysicsEntity> dynamicEntities;
	HashSet<MovablePhysicsEntity> movableEntities;
	HashSet<PhysicsProximityZone> proximityZones;

	// grid containing simulation entities
	HashGrid hashGrid;
	// types for hash grid entities
	static final int GRID_STATIC_COLLIDER_TYPE  = 0;
	static final int GRID_DYNAMIC_ENTITY_TYPE   = 1;
	static final int GRID_MOVABLE_ENTITY_TYPE   = 2;
	static final int GRID_PROXIMITY_ZONE_TYPE   = 3;

	// queues of entities to add
	ArrayList<DynamicPhysicsEntity> dynamicEntityAddQueue;
	ArrayList<MovablePhysicsEntity> movableEntityAddQueue;
	ArrayList<PhysicsProximityZone> proximityZoneAddQueue;
	void addQueued() {
		for (DynamicPhysicsEntity d : dynamicEntityAddQueue) {
			if (d.checkFlag(DynamicPhysicsEntity.FLAG_EXISTS)) {
				d.addToSimulation();
			}
		}
		for (MovablePhysicsEntity d : movableEntityAddQueue) {
			if (d.checkFlag(MovablePhysicsEntity.FLAG_EXISTS)) {
				d.addToSimulation();
			}
		}
		for (PhysicsProximityZone d : proximityZoneAddQueue) {
			if (d.checkFlag(PhysicsProximityZone.FLAG_EXISTS)) {
				d.addToSimulation();
			}
		}
		dynamicEntityAddQueue.clear();
		movableEntityAddQueue.clear();
		proximityZoneAddQueue.clear();
	}

	// queues of entities to delete
	ArrayList<DynamicPhysicsEntity> dynamicEntityDeleteQueue;
	ArrayList<MovablePhysicsEntity> movableEntityDeleteQueue;
	ArrayList<PhysicsProximityZone> proximityZoneDeleteQueue;
	void deleteQueued() {
		for (DynamicPhysicsEntity d : dynamicEntityDeleteQueue) {
			if (d.checkFlag(DynamicPhysicsEntity.FLAG_EXISTS)) {
				dynamicEntities.remove(d);
			}
		}
		for (MovablePhysicsEntity d : movableEntityDeleteQueue) {
			if (d.checkFlag(MovablePhysicsEntity.FLAG_EXISTS)) {
				movableEntities.remove(d);
			}
		}
		for (PhysicsProximityZone d : proximityZoneDeleteQueue) {
			if (d.checkFlag(PhysicsProximityZone.FLAG_EXISTS)) {
				proximityZones.remove(d);
			}
		}
		dynamicEntityDeleteQueue.clear();
		movableEntityDeleteQueue.clear();
		proximityZoneDeleteQueue.clear();
	}

	// array of category collision masks
	BitSet categoryCollisionMasks;
	// array of collision event handlers
	CollisionEventHandler[][] collisionEventHandlers;

	static final class TrackedIntersection {
		public PhysicsProximityZone proximityZone;
		public DynamicPhysicsEntity dynamicEntity;

		@Override
		public int hashCode() {
			return proximityZone.hashCode() ^ dynamicEntity.hashCode();
		}

		@Override
		public boolean equals(Object o) {
			if (o == null || !(o instanceof TrackedIntersection)) {
				return false;
			}
			TrackedIntersection other = (TrackedIntersection) o;
			return proximityZone == other.proximityZone && dynamicEntity == other.dynamicEntity;
		}
	}
	static final class TrackedIntersectionListNode {
		public PhysicsProximityZone proximityZone;
		public DynamicPhysicsEntity dynamicEntity;

		// next/prev pointers within proximity zone
		public TrackedIntersectionListNode proximityZonePrev, proximityZoneNext;
		// next/prev pointers within dynamic entity
		public TrackedIntersectionListNode dynamicEntityPrev, dynamicEntityNext;

		public void unlink() {
			// remove from each list
			if (proximityZone != null) {
				if (proximityZonePrev == null) {
					proximityZone.trackedIntersectionList = proximityZoneNext;
				} else {
					proximityZonePrev.proximityZoneNext = proximityZoneNext;
				}
				if (proximityZoneNext != null) {
					proximityZoneNext.proximityZonePrev = proximityZonePrev;
				}
			}
			if (dynamicEntity != null) {
				if (dynamicEntityPrev == null) {
					dynamicEntity.trackedIntersectionList = dynamicEntityNext;
				} else {
					dynamicEntityPrev.dynamicEntityNext = dynamicEntityNext;
				}
				if (dynamicEntityNext != null) {
					dynamicEntityNext.dynamicEntityPrev = dynamicEntityPrev;
				}
			}
			proximityZone = null;
			dynamicEntity = null;
		}

		// whether still intersecting
		boolean intersecting;
	}

	// proximity zone intersections tracked over multiple steps
	HashMap<TrackedIntersection, TrackedIntersectionListNode> trackedIntersections;
}
